import random
import time
import os
import sys
import curses
import _thread
from serial.tools import list_ports
from pydobot.dobot import Dobot

stdscr = curses.initscr()
curses.noecho()  # 不输出- -
curses.cbreak()  # 立刻读取:暂不清楚- -
stdscr.keypad(1)  # 开启keypad
stdscr.box()

width = os.get_terminal_size().columns
height = os.get_terminal_size().lines

stdscr.refresh()
key = ''
def get_input():
    global key
    key = stdscr.getch()
    print("yes")
    
# _thread.start_new_thread( get_input )
for i in range(0, 4):
    try:
        port = list_ports.comports()[i].device
        device = Dobot(port=port, verbose=False)
    except:
        pass
    else:
        break
# print(port)
if device == None:
    print("device not connectable")
    sys.exit()
device.suck(False)
device._set_queued_cmd_clear()
device.clear_all_alarms_state()
# device.go_home()
device.rotate_to(90,0,0,40,wait=True)

pump_enabled = False
move_control = False
rotate_control = False
while True:

    x,y,z,r,j1,j2,j3,j4 = device.pose()
    stdscr.addstr(1,1,"x= %.2f\t|" % x)
    stdscr.addstr(2,1,"y= %.2f\t|" % y)
    stdscr.addstr(3,1,"z= %.2f\t|" % z)
    stdscr.addstr(4,1,"r= %.2f\t|" % r)
    stdscr.addstr(5,1,"j1= %.2f\t|" % j1)
    stdscr.addstr(6,1,"j2= %.2f\t|" % j2)
    stdscr.addstr(7,1,"j3= %.2f\t|" % j3)
    stdscr.addstr(8,1,"j4= %.2f\t|" % j4)
    
    if move_control:
        stdscr.addstr(9,1,"i:x+ | k:x- | j:y+ | l:y- | u:z+ | o:z- | n:r+ | m:r-")
        if key == ord("i"):
            device.move_to(x+0.5,y,z,r)
        elif key == ord("k"):
            device.move_to(x-0.5,y,z,r)
        elif key == ord("j"):
            device.move_to(x,y+0.5,z,r)
        elif key == ord("l"):
            device.move_to(x,y-0.5,z,r)
        elif key == ord("u"):
            device.move_to(x,y,z+0.5,r)
        elif key == ord("o"):
            device.move_to(x,y,z-0.5,r)
        elif key == ord("n"):
            device.move_to(x,y,z,r+0.5)
        elif key == ord("m"):
            device.move_to(x,y,z,r-0.5)
        elif key == ord("q"):
            move_control=False
            break
        pass
    elif rotate_control:
        stdscr.addstr(9,1,"i:forward | k:backward | j:left | l:right | u:up | o:down")
        if key == ord("h"):
            device.go_home()
        elif key == ord("m"):
            move_control=True
        elif key == ord("r"):
            rotate_control=True
        elif key == ord("s"):
            device.suck(not pump_enabled)
            pump_enabled = not pump_enabled
        elif key == ord("q"):
            move_control=False
            break
        pass
    else:
        # os.system("clear")
        stdscr.addstr(9,1,"h:go home | q:quit | s:switch air pump | m:move mode | r:rotate mode ")
        if key == ord("h"):
            device.go_home()
        elif key == ord("m"):
            move_control=True
        elif key == ord("r"):
            rotate_control=True
        elif key == ord("s"):
            device.suck(not pump_enabled)
            pump_enabled = not pump_enabled
        elif key == ord("q"):
            break
    key = ''
    # key = stdscr.getch()
    stdscr.refresh()

curses.endwin()
